/*
car.h
offer car.h headfile ,api to outside
designed by linxiantao

-------------------------------
2020-05-07 15:20:00
create file
-------------------------------
*/

#ifndef __LIDAR_NAVIGATION_H
#define __LIDAR_NAVIGATION_H

#include "base_type.h"
#include "../driver/lidar/bea_u951.h"
#include "con_algorithm.h"
#include "../washer/station/washwork.h"
#include "../washer/project_config.h"
#ifdef __cplusplus
extern "C" {
#endif

//机器人朝向角度是否采用管线架编码器
#define ROBOT_ANGLE_USE_JOINT_ENCODER

//直角洗车顶找平后是否采用joint_move
#define USE_RECTANGULAR_JOINT_MOVE

#define ROBOT_LENGTH_VALUE 120//机器长度，单位cm
#define ROBOT_WIDTH_VALUE 60//机器宽度，单位cm
#define ROBOT_SAFE_DISTANCE_VALUE (ROBOT_WIDTH_VALUE + 10)//离车安全距离：机器宽度+10cm
#define ROBOT_ROUND_SIDE_TIME_OUT (5*60*1000)//绕边超时时间5分钟

#define LIDAR_X_BASE_ROBOT_COORDINATE (64)//雷达中心点基于机器人坐标系的X坐标值，单位cm
#define LIDAR_Y_BASE_ROBOT_COORDINATE (-28)//雷达中心点基于机器人坐标系的Y坐标值，单位cm

#ifdef USE_RECTANGULAR_JOINT_MOVE
#define TRAJECTORY_POINT_INTERVAL 1000//规划轨迹点步距，单位mm
#endif

typedef enum{
	WHEEL_L = 0,
	WHEEL_R,
	WHEEL_NUMS
}WHEEL_ENUM;

//robot pose object
typedef struct{
	int x;	//单位mm
	int y;	//单位mm
	int th_odom;//由里程计计算得到的机器人朝向角，单位1/1000度
	int th_gyro;//由陀螺仪计算得到的机器人朝向角，单位1/1000度
}Robot_pose_object;

//odometry object
typedef struct{
	uint32_t calc_cnt;
	uint64_t encode_now[WHEEL_NUMS];
	uint64_t encode_last[WHEEL_NUMS];
	int32_t robot_init_angle;//机器人初始朝向角，单位1/1000度
	int32_t robot_angle;//由陀螺仪计算得到的机器人当前朝向角度，单位1/1000度
	int32_t robot_angle_odom;//由里程计计算得到的机器人当前朝向角度，单位1/1000度
	float vel_x;
	float vel_y;
	float vel_z;
	float x;//世界坐标X，单位mm
	float y;//世界坐标Y，单位mm
	float z;//由陀螺仪计算得到的机器人朝向角，单位弧度
	float z_odom;//由里程计计算得到的机器人朝向角，单位弧度
	uint64_t mileage_mm;//总里程，不分前后，单位mm
}Odom_class;

//lidar_navi object
typedef struct{
	PIDtypedef pid;//pid描述符
	float kp;//pid的p值
	float ki;//pid的i值
	float kd;//pid的d值
	float robot_base_speed;//机器人pid绕行基准速度
	float robot_speed_max;//机器人pid绕行最大速度
	float robot_pid_offset;//机器人pid绕行补偿值，由毛刷压力检测引入
	float mean_square_error_value;		//找平的均方根误差值
	int mean_square_index;	//找平数组索引
	u8 left_upper_io_num;//机器人左边超声波信号
	u8 right_upper_io_num;//机器人右边超声波信号
	int32_t robot_safe_distance;//机器人pid绕行阈值
	int32_t robot_round_side_end;//机器人pid绕边是否完成
	int32_t robot_turning_end;//机器人pid转弯是否完成
	int32_t lidar_open_flag;//机器人雷达是否打开
	int8_t robot_round_side_stop;//机器人pid绕行停止
	int8_t robot_round_side;//绕哪条边
	int8_t robot_turning_type;//转弯类型
	int32_t is_robot_straightening_end;//是否摆正完成
	int32_t is_robot_round_side_leave_car;//是否绕边离开车
	uint8_t b_odom_enable;
	Odom_class odom;//里程计 
}lidar_navigation_object;

int32_t xp_is_robot_straightening_end();
void xp_reset_robot_straightening_end();

extern int xp_navi_lidar_get(void);		//get lidar distance cm
extern int xp_navi_lidar_get2(void);
extern int xp_navi_lidar_get3(void);
void xp_reset_robot_round_side();
void xp_robot_round_side(int8_t side, uint32_t timeout);
void xp_robot_turning(int8_t type);
int32_t xp_is_round_side_leave_car();

int32_t xp_is_robot_turning_end();
void xp_reset_robot_turning();
bool xp_is_robot_search_car();
bool xp_is_robot_leave_car();
bool is_single_robot_mode(void);
void set_car_head_x_position(int32_t x_position);
void set_car_tail_x_position(int32_t x_position);
int32_t get_car_left_parallel_y(void);
int32_t get_car_left_parallel_x(void);
int32_t get_car_right_parallel_y(void);
int32_t get_car_right_parallel_x(void);
void xp_set_robot_safe_distance();
void xp_robot_round_side_time(int32_t time);
void xp_set_robot_round_side(int8_t side);
void xp_round_side_signal();
void xp_set_robot_turning_type(int8_t type);
void xp_turning_signal();

int32_t lidar_navigation_recover_wash_param_init();
// void set_emergency_status_flag(bool val);
// bool get_emergency_status_flag();
void set_robot_base_speed(float val);
void set_robot_pid_offset(float val);
int32_t xp_lidar_navigation_init();
float get_robot_angle();
float get_odom_angle();
float get_robot_angle_zero_offset();
int32_t get_robot_pose(ROBOT_POSE *pose);
int32_t update_another_robot_pose(ROBOT_POSE pose, u8 side);
int32_t get_another_robot_pose(ROBOT_POSE *pose);
int32_t get_another_robot_side(CAR_SIDE_E *side);

void set_round_test_end_flag(bool val);
void xp_robot_round_test();
void set_wash_car_roof_flag(bool val);
void set_pipe_collision_enable_flag(bool val);

int32_t xp_is_robot_round_side_reset_gyro_end();
void xp_reset_robot_round_side_reset_gyro();
void xp_round_side_reset_gyro_signal();
void set_round_side_reset_gyro_timeout(int32_t val);
void set_robot_straightening_side(int32_t side);
void xp_robot_right_angle_signal();
int xp_car_leave_state(void);
void xp_leave_sem_set(uint8_t type);
void xp_odom_start_signal();
Odom_class *xp_odom_object_get(void);
extern int xp_navigation_debug(char *type,char *fun,char *param);

#ifdef __cplusplus
}
#endif


#endif

